#ifdef _WIN32
#include <windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>

#include <GL/glew.h>
#include "textfile.h"

#ifndef __APPLE__
#include <GL/gl.h>
#include <GL/glut.h>
#else
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#endif

#include <AR/gsub.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/ar.h>

//
// Camera configuration.
//
#ifdef _WIN32
char            *vconf = "Data\\WDM_camera_flipV.xml";
#else
char            *vconf = "-dev=/dev/video1";
#endif

int             xsize, ysize;
int             thresh = 100;
int             count = 0;

char           *cparam_name    = "Data/camera_para.dat";
ARParam         cparam;

char           *patt_name      = "Data/patt.hiro";
int             patt_id;
double          patt_width     = 80.0;
double          patt_center[2] = {0.0, 0.0};
double          patt_trans[3][4];

static void   init(void);
static void   cleanup(void);
static void   keyEvent( unsigned char key, int x, int y);
static void   mainLoop(void);
static void   draw( void );

////////////////////////NEW CODE////////////////////////////////////////
// Handles for the shader programs
GLhandleARB v,f,p;
// Handle for the webcam feed texture
GLuint webcam_texture;
int webcam_sampler;
int sx=-4, sy=80, sz=37, sr=22;

void printSphereValues()
{
    printf("sx = %d, sy = %d, sz = %d, sr = %d", sx, sy, sz, sr);
}

void printInfoLog(GLhandleARB obj)
{
    int infologLength = 0;
    int charsWritten  = 0;
    char *infoLog;
    
    glGetObjectParameterivARB(obj, GL_OBJECT_INFO_LOG_LENGTH_ARB,
                         &infologLength);
    
    if (infologLength > 0)
    {
        infoLog = (char *)malloc(infologLength);
        glGetInfoLogARB(obj, infologLength, &charsWritten, infoLog); 
        printf("%s\n",infoLog);
        free(infoLog);
    }
}

void checkGLErrors()
{
    if (glGetError()!=GL_NO_ERROR)
       printf("ERROR!!!!\n");
}

void setShaders() {

    char *vs,*fs,*fs2;

    v = glCreateShaderObjectARB(GL_VERTEX_SHADER_ARB);
    f = glCreateShaderObjectARB(GL_FRAGMENT_SHADER_ARB);

    vs = textFileRead("vert.glsl");
    fs = textFileRead("frag.glsl");

    const char * ff = fs;
    const char * vv = vs;

    glShaderSourceARB(v, 1, &vv,NULL);
    glShaderSourceARB(f, 1, &ff,NULL);

    free(vs);free(fs);

    glCompileShaderARB(v);

    printInfoLog(v);

    glCompileShaderARB(f);
    printInfoLog(f);

    p = glCreateProgramObjectARB();
    glAttachObjectARB(p,f);
    glAttachObjectARB(p,v);

    glLinkProgramARB(p);
    glUseProgramObjectARB(p);

    glGetUniformLocationARB(p, "tmp_color");
    glGetUniformLocationARB(p, "webcam");
}

////////////////////////////////////////////////////////////////////////

int main(int argc, char **argv)
{
    glutInit(&argc, argv);

    init();

     //NEWCODE/////
     glewInit();
    if (GLEW_ARB_vertex_shader && GLEW_ARB_fragment_shader)
        printf("Ready for GLSL\n");
    else {
        printf("No GLSL support\n");
        exit(1);
    }
    setShaders();
     //////////////

    arVideoCapStart();

    argMainLoop( NULL, keyEvent, mainLoop );
    return (0);
}

static void   keyEvent( unsigned char key, int x, int y)
{
    /* quit if the ESC key is pressed */
    if( key == 0x1b ) {
        printf("*** %f (frame/sec)\n", (double)count/arUtilTimer());
        cleanup();
        exit(0);
    }
    switch(key)
    {
    case 'a':
        sx-=1.0;
        break;
    case 'd':
        sx+=1.0;
        break;
    case 'w':
        sy+=1.0;
        break;
    case 's':
        sy-=1.0;
        break;
    case 'r':
        sz+=1.0;
        break;
    case 'f':
        sz-=1.0;
        break;
    case 't':
        sr+=1.0;
        break;
    case 'g':
        sr-=1.0;
        break;
    case 'p':
        printSphereValues();
    }
        
}

/* main loop */
static void mainLoop(void)
{
    ARUint8         *dataPtr;
    ARMarkerInfo    *marker_info;
    int             marker_num;
    int             j, k;

    /* grab a vide frame */

    if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
        arUtilSleep(2);
        return;
    }

    if( count == 0 ) arUtilTimerReset();
    count++;

    //NEWCODE////////////////
    glUseProgramObjectARB(0);
    /////////////////////////

    argDrawMode2D();
    argDispImage( dataPtr, 0,0 );
    
    glEnable(GL_TEXTURE_2D);
    glBindTexture( GL_TEXTURE_2D, webcam_texture);
    //glTexImage2D( GL_TEXTURE_2D, 0, 3, xsize, ysize, 0, GL_RGB, GL_UNSIGNED_BYTE, dataPtr);
    //glCopyTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 0, 0, 512, 512, 0);
    glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, 256, 256); 

    checkGLErrors();

    /* detect the markers in the video frame */
    if( arDetectMarker(dataPtr, thresh, &marker_info, &marker_num) < 0 ) {
        cleanup();
        exit(0);
    }

    arVideoCapNext();

    /* check for object visibility */
    k = -1;
    for( j = 0; j < marker_num; j++ ) {
        if( patt_id == marker_info[j].id ) {
            if( k == -1 ) k = j;
            else if( marker_info[k].cf < marker_info[j].cf ) k = j;
        }
    }
    if( k == -1 ) {
        argSwapBuffers();
        return;
    }

    /* get the transformation between the marker and the real camera */
    arGetTransMat(&marker_info[k], patt_center, patt_width, patt_trans);
    
    draw();

    argSwapBuffers();
}

static void init( void )
{
    ARParam  wparam;
    
    /* open the video path */
    if( arVideoOpen( vconf ) < 0 ) exit(0);
    /* find the size of the window */
    if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
    printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);

    /* set the initial camera parameters */
    if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
        printf("Camera parameter load error !!\n");
        exit(0);
    }
    arParamChangeSize( &wparam, xsize, ysize, &cparam );
    arInitCparam( &cparam );
    printf("*** Camera Parameter ***\n");
    arParamDisp( &cparam );

    if( (patt_id=arLoadPatt(patt_name)) < 0 ) {
        printf("pattern load error !!\n");
        exit(0);
    }

    /* open the graphics window */
    argInit( &cparam, 1.0, 0, 0, 0, 0 );

    //Create the webcam_texture
    glEnable(GL_TEXTURE_2D);
    glGenTextures(1, &webcam_texture);
    glBindTexture(GL_TEXTURE_2D, webcam_texture);
    glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
    glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
    //glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT );
    //glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT );
    unsigned int *data = (unsigned int*)malloc(((256 * 256)* 3 * sizeof(unsigned int)));
    glTexImage2D(GL_TEXTURE_2D, 0, 3, 256, 256, 0, GL_RGB, GL_UNSIGNED_BYTE, data);
    free(data);
}

/* cleanup function called when program exits */
static void cleanup(void)
{
    //Delete webcam texture
    glDeleteTextures(1, &webcam_texture);
    arVideoCapStop();
    arVideoClose();
    argCleanup();
}

static void draw( void )
{
    double    gl_para[16];
    GLfloat   mat_ambient[]     = {0.0, 0.0, 1.0, 1.0};
    GLfloat   mat_flash[]       = {0.0, 0.0, 1.0, 1.0};
    GLfloat   mat_flash_shiny[] = {50.0};
    GLfloat   light_position[]  = {100.0,-200.0,200.0,0.0};
    GLfloat   ambi[]            = {0.1, 0.1, 0.1, 0.1};
    GLfloat   lightZeroColor[]  = {0.9, 0.9, 0.9, 0.1};
	GLfloat   lightDebugColor[]  = {0, 1, 0, 0.1};

    glUseProgramObjectARB(p);
    glActiveTexture(GL_TEXTURE0);
    glUniform1iARB(p, 0);
 
   
    argDrawMode3D();
    argDraw3dCamera( 0, 0 );
    glClearDepth( 1.0 );
    glClear(GL_DEPTH_BUFFER_BIT);
    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LEQUAL);
    
    /* load the camera transformation matrix */
    argConvGlpara(patt_trans, gl_para);
    glMatrixMode(GL_MODELVIEW);
    glLoadMatrixd( gl_para );

    glEnable(GL_TEXTURE_2D);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);
    glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
    glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
    glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);  
    glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
    glMatrixMode(GL_MODELVIEW);
	/*glPushMatrix();
    glTranslatef( 0.0, 0.0, 25.0 );
    glutSolidCube(50.0);
	glPopMatrix();*/
	//get cam vectors
	GLfloat right[3], up[3];
	{
		right[0] = gl_para[0];
		right[1] = gl_para[4];
		right[2] = gl_para[8];

		up[0] = gl_para[1];
		up[1] = gl_para[5];
		up[2] = gl_para[9];

	}

	static float tx = -1;
	static float ty = 76;
	static float tz = 38;
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightDebugColor);
	glPushMatrix();
	//
	glTranslatef( tx, ty, tz );
	glScalef(21,21,21);
	//glutSolidSphere(3, 20, 20);
	
	glBegin( GL_QUADS );
	glVertex3d( right[0] + up[0], right[1] + up[1], right[2] + up[2] );
	glVertex3d( right[0] - up[0], right[1] - up[1], right[2] - up[2] );
	glVertex3d( -right[0] - up[0], -right[1] - up[1], -right[2] - up[2] );
	glVertex3d( -right[0] + up[0], -right[1] + up[1], -right[2] + up[2] );
	glEnd();
	glPopMatrix();
	


	glPushMatrix();
	glLoadIdentity();
	glTranslatef( gl_para[12], gl_para[13], gl_para[14] );
	 glBegin(GL_LINES);
       glVertex3f( 0, 0, 0 );
       glVertex3f( gl_para[0]*100, gl_para[1]*100, gl_para[2]*100 );
       glVertex3f( 0, 0, 0 );
       glVertex3f( gl_para[4]*100, gl_para[5]*100, gl_para[6]*100 );
       glVertex3f( 0, 0, 0 );
       glVertex3f( gl_para[8]*100, gl_para[9]*100, gl_para[10]*100 );
	   glColor3f(0,1,0);
	   glVertex3f( 0, 0, 0 );
       glVertex3f( (gl_para[4] + gl_para[8] + gl_para[0])*33, 
				   (gl_para[5] + gl_para[9] + gl_para[1])*33, 
				   (gl_para[10] + gl_para[6] + gl_para[2])*33 );	
     glEnd();
	 glPopMatrix();
	glDisable( GL_LIGHTING );
    glDisable( GL_DEPTH_TEST );
}
